#include "SearcherConfig.h"
#include <fstream>
#include <iostream>
#include "Utility.h"
#include "DecoLogger.h"

JointBoundType SplitTypeAndName(const std::string& rawName, std::string& jointName)
{
	JointBoundType type;
	if (!strncmp(rawName.c_str(), "amplitude", 9))
	{
		type = JBT_Amplitude;
		jointName = rawName.substr(10, rawName.size());
	}
	else if (!strncmp(rawName.c_str(), "phase", 5))
	{
		type = JBT_Phase;
		jointName = rawName.substr(6, rawName.size());
	}
	else if (!strncmp(rawName.c_str(), "constant", 8))
	{
		type = JBT_Constant;
		jointName = rawName.substr(9, rawName.size());
	}
	else if (!strncmp(rawName.c_str(), "min", 3))
	{
		type = JBT_Min;
		jointName = rawName.substr(4, rawName.size());
	}
	else if (!strncmp(rawName.c_str(), "max", 3))
	{
		type = JBT_Max;
		jointName = rawName.substr(4, rawName.size());
	}
	else if (!strncmp(rawName.c_str(), "straight1", 9))
	{
		type = JBT_StraightLen1;
		jointName = rawName.substr(10, rawName.size());
	}
	else if (!strncmp(rawName.c_str(), "straight2", 9))
	{
		type = JBT_StraightLen2;
		jointName = rawName.substr(10, rawName.size());
	}
	else if (!strncmp(rawName.c_str(), "half", 4))
	{
		type = JBT_HalfRatio;
		jointName = rawName.substr(5, rawName.size());
	}
	else if (!strncmp(rawName.c_str(), "warp0", 5))
	{
		type = JBT_Warp0;
		jointName = rawName.substr(6, rawName.size());
	}
	else if (!strncmp(rawName.c_str(), "warp1", 5))
	{
		type = JBT_Warp1;
		jointName = rawName.substr(6, rawName.size());
	}
	else if (!strncmp(rawName.c_str(), "warp2", 5))
	{
		type = JBT_Warp2;
		jointName = rawName.substr(6, rawName.size());
	}
	else if (!strncmp(rawName.c_str(), "warp3", 5))
	{
		type = JBT_Warp3;
		jointName = rawName.substr(6, rawName.size());
	}

	return type;
}


SceneBuilderInfo::SceneBuilderInfo()
{

}

SceneBuilderInfo::SceneBuilderInfo(const std::string modelName, const vector3& boxMin, const vector3& boxMax, int resolution, int simDim) :    
mSceneBox(boxMin, boxMax),
mResolution(resolution),
mSimulationDim(simDim)
{
	mModelFileName = "../models/swim/" + modelName + ".skel";
}

JointBound::JointBound()
{

}

JointBound::JointBound(const std::string& tobeParsed, double minValue, double maxValue) : mRawName(tobeParsed), mMinValue(minValue), mMaxValue(maxValue)
{
	mType = SplitTypeAndName(tobeParsed, mJointName);
}

SearcherInfo::SearcherInfo() : mObjFuncId(0), mSearchTimeLength(2.0), mbEnergyPrune(false), mbCollisionDetection(false), mUseDegree(false), mbEvalSecondCycle(false), mEnergyBound(200.0), mCMAInitialPath("../initials.par"), mCMASignalPath("../signals.par")
{
	for(int i = 0; i < 10; i++)
		mIdxEnergyBounds[i] = 0.0;
}

SearcherInfo::SearcherInfo(int objFuncId, double searchTimeLen) : mObjFuncId(objFuncId), mSearchTimeLength(searchTimeLen), mbEnergyPrune(false), mbCollisionDetection(false), mUseDegree(false), mbEvalSecondCycle(false), mEnergyBound(200.0), mCMAInitialPath("../initials.par"), mCMASignalPath("../signals.par")
{
	for(int i = 0; i < 10; i++)
		mIdxEnergyBounds[i] = 0.0;
}


void SearcherInfo::AddJointConstraint(const JointSearchConstraint& constraint)
{
	mJointConstraints.push_back(constraint);
}

void SearcherInfo::AddJointConstraint(const std::string& rawName1, const std::string& rawName2, const std::string& type)
{
	JointSearchConstraint constraint;
	constraint.mRawName1 = rawName1;
	constraint.mRawName2 = rawName2;
	if (type == "equal")
		constraint.mType = JSCT_Equal;
	else if (type == "negate")
		constraint.mType = JSCT_Negate;
	else if (type == "less")
		constraint.mType = JSCT_Less;
	else if (type == "greater")
		constraint.mType = JSCT_Greater;
	constraint.mBoundType = SplitTypeAndName(rawName1, constraint.mJointName1);
// 	JointBoundType boundType2 = SplitTypeAndName(rawName2, constraint.mJointName2);
// 	if (boundType2 != constraint.mBoundType)
// 	{
// 		printf("Bound type in constraint do not match between %s and %s\n", rawName1.c_str(), rawName2.c_str());
// 		(*DecoLogger::GetSingleton()) << "Bound type in constraint do not match between " << rawName1 << " and " << rawName2 << "\n";
// 		return;
// 	}
	int jointBoundSize = static_cast<int>(mJointBounds.size());
	constraint.mConstraintedJointIdx = -1;
	for (int i = 0; i < jointBoundSize; ++i)
	{
		if (rawName1 == mJointBounds[i].mRawName)
		{
			constraint.mConstraintedJointIdx = i;
			break;
		}
	}

	constraint.mConstraintToJointIdx = -1;
	for (int i = 0; i < jointBoundSize; ++i)
	{
		if (rawName2 == mJointBounds[i].mRawName)
		{
			constraint.mConstraintToJointIdx = i;
			break;
		}
	}

	if (constraint.mConstraintedJointIdx == -1 && (constraint.mType == JSCT_Less || constraint.mType == JSCT_Greater))
	{
		assert(0);
		return;
	}
	if (constraint.mConstraintToJointIdx == -1)
	{
		printf("Constrained to joint %s not found for %s\n", rawName2.c_str(), rawName1.c_str());
		(*DecoLogger::GetSingleton()) << "Constrained to joint " << rawName2 << " not found for " << rawName1 << "\n";
		return;
	}

	mJointConstraints.push_back(constraint);
}

void SearcherInfo::AddJointBound(const JointBound& bound)
{
	mJointBounds.push_back(bound);
}
void SearcherInfo::AddJointBound(const std::string& rawName, double minValue, double maxValue)
{
	mJointBounds.push_back(JointBound(rawName, minValue, maxValue));
}



void SearcherConfig::Init(const string& filename)
{
  (*DecoLogger::GetSingleton()) << "Start initialize the SearcherConfig from file " << filename << "\n";
	mConfigFileName = filename;
	ifstream fileReader(filename.c_str());
	char buffer[512];
	while(!fileReader.eof())
	{
		memset(buffer, 0, 512 * sizeof(char));
		fileReader.getline(buffer, 512);
  (*DecoLogger::GetSingleton()) << "Start parsing buffer " << buffer << "\n";

		parse(buffer);
	}
}

void SearcherConfig::parse(const string& buffer)
{
  std::string bufferUnix = buffer;
  int strLen = static_cast<int>(bufferUnix.size());
  if (!strLen)
	  return;
  if (bufferUnix[strLen - 1] == '\r')
    {
      bufferUnix = bufferUnix.substr(0, strLen - 1);
    }
	std::vector<std::string> tokens;
	std::cout << "buffer is " << bufferUnix << std::endl;
	Tokenize(bufferUnix, tokens);
	//	(*DecoLogger::GetSingleton()) << "token size is " << static_cast<int>(tokens.size()) << "\n";
	//	(*DecoLogger::GetSingleton()) << "token[0] is " << tokens[0] << "\n";
	std::cout << "tokens size is " << tokens.size() << std::endl;
	for (int i = 0; i < tokens.size(); ++i)
	  {
	    std::cout << tokens[i];
	  }
	std::cout << std::endl;
	if (tokens.size())
	{
		if (!strncmp(tokens[0].c_str(), "//", 2))
		{

		}
		else if (tokens[0] == "[SceneBuilder]")
		{
			checkConfigError(tokens, 1);
		}
		else if (tokens[0] == "[Searcher]")
		{
			checkConfigError(tokens, 1);
		}
		else if (tokens[0] == "sceneBox")
		{
			checkConfigError(tokens, 3);
			vector3 boxMin, boxMax;
			if (!parseVector3(tokens[1].c_str(), boxMin) || !parseVector3(tokens[2].c_str(), boxMax))
			{
				printf("Cannot parse %s or %s into vector3!\n", tokens[1].c_str(), tokens[2].c_str());
				(*DecoLogger::GetSingleton()) << "Cannot parse " << tokens[1] << " or " << tokens[2] << " into vector3!\n";
				return;
			}
			mSceneBuilderInfo.mSceneBox = Box(boxMin, boxMax);
		}
		else if (tokens[0] == "resolution")
		{
			checkConfigError(tokens, 2);
			mSceneBuilderInfo.mResolution = atof(tokens[1].c_str());
		}
		else if (tokens[0] == "model")
		{
			checkConfigError(tokens, 2);
			mSceneBuilderInfo.mModelFileName = "../models/swim/" + tokens[1] + ".skel";
		}
		else if (tokens[0] == "simulationDim")
		{
			checkConfigError(tokens, 2);
			mSceneBuilderInfo.mSimulationDim = atoi(tokens[1].c_str());
		}
		else if (tokens[0] == "objectiveFunc")
		{
			checkConfigError(tokens, 2);
			mSearcherInfo.mObjFuncId = atoi(tokens[1].c_str());
		}
		else if (tokens[0] == "useDegrees")
		{
			checkConfigError(tokens, 2);
			mSearcherInfo.mUseDegree = atoi(tokens[1].c_str());
		}
		else if (tokens[0] == "searchTimeLength")
		{
			checkConfigError(tokens, 2);
			mSearcherInfo.mSearchTimeLength = atof(tokens[1].c_str());
		}
		else if (tokens[0] == "energyBound")
		{
			checkConfigError(tokens, 2);
			mSearcherInfo.mEnergyBound = atof(tokens[1].c_str());
		}
		else if (tokens[0] == "indexedEnergyBound")
		{
			checkConfigError(tokens, 3);
			int iIdx = atoi(tokens[1].c_str());
			mSearcherInfo.mIdxEnergyBounds[iIdx] = atof(tokens[2].c_str());
		}
		else if (tokens[0] == "energyPrune")
		{
			checkConfigError(tokens, 2);
			mSearcherInfo.mbEnergyPrune = atoi(tokens[1].c_str());
		}
		else if (tokens[0] == "collisionDetection")
		{
			checkConfigError(tokens, 2);
			mSearcherInfo.mbCollisionDetection = atoi(tokens[1].c_str());
		}
		else if (tokens[0] == "evalSecondCycle")
		{
			checkConfigError(tokens, 2);
			mSearcherInfo.mbEvalSecondCycle = atoi(tokens[1].c_str());
		}
		else if (tokens[0] == "cmaInitial")
		{
			checkConfigError(tokens, 2);
			mSearcherInfo.mCMAInitialPath = tokens[1];
		}
		else if (tokens[0] == "cmaSignal")
		{
			checkConfigError(tokens, 2);
			mSearcherInfo.mCMASignalPath = tokens[1];
		}
		else if (tokens[0] == "constraint")
		{
			checkConfigError(tokens, 4);
			mSearcherInfo.AddJointConstraint(tokens[2], tokens[3], tokens[1]);
		}
		else if (tokens[0] == "params")
		{
			int numJointBounds = static_cast<int>(mSearcherInfo.mJointBounds.size());
			for (int i = 0; i < tokens.size() - 1; ++i)
			{
				float fRad = atof(tokens[i + 1].c_str());
				if(mSearcherInfo.mUseDegree)
					fRad = fRad / 180.0f * 3.14159265f;
				mSearcherInfo.mJointParams.push_back(fRad);
			}
		}
		else 
		{
			checkConfigError(tokens, 3);
			float fRad1 = atof(tokens[1].c_str()), fRad2 = atof(tokens[2].c_str());
			if(mSearcherInfo.mUseDegree)
			{
				fRad1 = fRad1 / 180.0f * 3.14159265f;
				fRad2 = fRad2 / 180.0f * 3.14159265f;
			}
			mSearcherInfo.AddJointBound(tokens[0], fRad1, fRad2);
		}

	}
}

void SearcherConfig::checkConfigError(const std::vector<string>& tokens, size_t numInput)
{
	if (numInput != tokens.size())
	{
		printf("Searcher config error: %s should has %d inputs but only %d is given!\n", tokens[0].c_str(), numInput, tokens.size());
		(*DecoLogger::GetSingleton()) << "Searcher config error: " << tokens[0] << " should has " << static_cast<int>(numInput) << " inputs but only " << static_cast<int>(tokens.size()) << " is given!\n";
	}
}

SearcherConfig::SearcherConfig()
{

}
SearcherConfig::~SearcherConfig()
{

}


bool SearcherConfig::parseVector3(const char* Text, vector3& Value) const
{
	double vectorValue[3];
	string potentialVector(Text);
	const char* start = strchr(Text, '(');
	if (!start)
		return false;
	start++;
	for (INT i = 0; i < 2; i++)
	{
		const char* comma = strchr(start, ',');
		if (!comma)
			return false;
		UINT numChar = static_cast<UINT>(comma - start);
		UINT startPos = static_cast<UINT>(start - Text);
		string value = potentialVector.substr(startPos, numChar);
		vectorValue[i] = atof(value.c_str());
		start = comma + 1;
	}
	const char* end = strchr(Text, ')');
	if (!end)
		return false;
	UINT numChar = static_cast<UINT>(end - start);
	UINT startPos = static_cast<UINT>(start - Text);
	string value = potentialVector.substr(startPos, numChar);
	vectorValue[2] = atof(value.c_str());
	Value.x = vectorValue[0];
	Value.y = vectorValue[1];
	Value.z = vectorValue[2];
	return true;

}

const SceneBuilderInfo& SearcherConfig::GetSceneBuilderInfo() const
{
	return mSceneBuilderInfo;
}

const SearcherInfo& SearcherConfig::GetSearcherInfo() const
{
	return mSearcherInfo;
}
